// check out r64 of this file to let you see a bunch load of cool things 


#include "Gesture.h"
#include "MatrixUtils.h"
#include "Regression.h" 

#define SPEED_THRESH 3
#define BETA 0.1  // reference trajectory attractor parameter 


Matrix Gesture::add_time(Matrix& demo)
{
  int csize = demo.ColumnSize();
  int rsize = demo.RowSize();
  Matrix ndemo(rsize,csize+1);
  Vector row(csize);
  Vector nrow(csize+1);
  float time_step = ((float) n_resample) / rsize;
  for(int i =0;i<rsize;i++)
    {
      row = demo.GetRow(i);
      nrow(0) = i*time_step;
      nrow.SetSubVector(1,row);
      ndemo.SetRow(nrow,i);
    }
  return ndemo;
}


Matrix Gesture::computeVelocity(Matrix posData)
{
  Matrix result;
 
  result.Resize(posData.RowSize(),(posData.ColumnSize()-1)*2+1);
  result.SetColumnSpace(posData,0);

  for(unsigned int i=0;i<posData.RowSize()-1;i++){
    float deltaT = posData(i+1,0) - posData(i,0);
    for(unsigned int k=0;k<(posData.ColumnSize()-1);k++){
      result(i,posData.ColumnSize()+k)=(posData(i+1,k+1)-posData(i,k+1))/deltaT;
    } 
  }
  result.Resize(posData.RowSize(),result.ColumnSize());

  return result;
}


Matrix Gesture::computeVelocity_notime(Matrix posData)
{
  Matrix result;
 
  result.Resize(posData.RowSize(),(posData.ColumnSize())*2+1);
  result.SetColumnSpace(posData,1);

  for(unsigned int i=0;i<posData.RowSize()-1;i++){
    float deltaT = 1;
   result(i,0) = i;
    for(unsigned int k=0;k<posData.ColumnSize();k++){
      result(i,posData.ColumnSize()+k+1)=(posData(i+1,k)-posData(i,k))/deltaT;
    } 
  }
  result.Resize(posData.RowSize(),result.ColumnSize());
  return result;
}

Gesture::Gesture(int n_ref,int nstates,int nbData,int n_resample)
{
  nObjects = n_ref;
  demos = new Matrix*[nObjects];
  ndemos = new int[nObjects];
  gmms = new  GaussianMixture*[nObjects];
  this->n_resample = n_resample;
  this->nstates = nstates;
  this->nbData = nbData;
  demo_has_time=0; 
  nbJoints=0;
}



void Gesture::saveParams(char filename[],char filename_joints[])
{
    saveParams(filename); 
    if(nbJoints > 0)
      g_joints.saveParams(filename_joints);
}

void Gesture::saveParams(char filename[])
{

  char cfilename[100];
  for(int i=0;i<nObjects;i++)
    {
      sprintf(cfilename,"%s%02d",filename,i);
      std::cout << cfilename << std::endl;
      gmms[i]->saveParams(cfilename);
    }
}

void Gesture::loadParams(char filename[],char filename_joints[])
{
  loadParams(filename);
  nbVar = gmms[0]->dim;
  nbVarPos = (nbVar -1)/2;
  std::cout << "gmm imported from "<<filename << std::endl;
  if(strcmp(filename_joints,""))
    {
      g_joints.loadParams(filename_joints);
      this->nbJoints = g_joints.dim - 1;
    }
}

void Gesture::loadParams(char filename[])
{
  char cfilename[100];
  for(int i=0;i<nObjects;i++)
    {
      sprintf(cfilename,"%s%02d",filename,i);
      gmms[i] = new GaussianMixture();
      gmms[i]->loadParams(cfilename);
    }
  nbVar = gmms[0]->dim;
  nbVarPos = (nbVar-1)/2;
}


void Gesture::setdemos(Matrix * raw, Matrix * joints,int n,int currentRef)
{
  setdemos(raw,n,currentRef);
  this->nbJoints = joints[0].ColumnSize();
  joints_demos = new Matrix[n];
  for(int i=0;i<n;i++)
    {
      Matrix tmp;
      if(demo_has_time)
	{
	  Regression::HermitteSplineFit(joints[i],n_resample,tmp);
	}
      else
	{
	  Matrix timed = add_time(joints[i]);
	  /*Matrix timed(joints[i].RowSize(),this->nbJoints+1);
	  for(unsigned int k=0;k<joints[i].RowSize();k++)
	    timed(k,0)=(float) k;
	    timed.SetColumnSpace(joints[i],1);*/
	  Regression::HermitteSplineFit(timed,n_resample,tmp);
	  timed.Print();
	}
      joints_dataset = tmp.VCat(joints_dataset);
    }
}
  

void Gesture::setdemos(Matrix * raw,int n,int currentRef)
{
  if(currentRef >= nObjects)
    {
      std::cout << "RelativeGesture :: wrong current Reference" << std::endl;
    }
  demos[currentRef] = new Matrix[n];
  if(demo_has_time)
    {
      nbVarPos = raw[0].ColumnSize() - 1 ;     
    }
  else
    {
      nbVarPos = raw[0].ColumnSize();
    }
 nbVar = nbVarPos*2 +1;
   for(int i=0;i<n;i++)
    {
      Matrix interpol;
      Matrix tmp;

#ifdef USE_DTW 

      if(i>0)
	{
	  if(demo_has_time)
	    DTW(raw[0].GetColumnSpace(1,nbVarPos),
		raw[i].GetColumnSpace(1,nbVarPos),
		tmp);
	  else
	    DTW(raw[0],raw[i],tmp);
	}
      else
	{
	  if(demo_has_time) tmp = raw[i].GetColumnSpace(1,nbVarPos);
	  else tmp = add_time(raw[i]);
	}

#else

      if(demo_has_time)
	tmp = raw[i].GetColumnSpace(1,nbVarPos);
      else 
	tmp = add_time(raw[i]);

#endif
      interpol.Resize(n_resample,nbVar);
 
      if((int)tmp.RowSize() != n_resample)
	{
	  Regression::HermitteSplineFit(tmp,n_resample,interpol); 
	}
      else
	interpol = tmp;

      tmp = computeVelocity(interpol);
      demos[currentRef][i] = tmp; 
      //demos[currentRef][i].Print();
    }  
   ndemos[currentRef] = n;
}


void Gesture::train()
{
  Matrix dataset;
  for(int i=0;i<nObjects;i++)
    {
      dataset.Resize(0,0);
      gmms[i] = new GaussianMixture();
      
      for(int j=0;j<ndemos[i];j++)
	{
	  dataset = demos[i][j].VCat(dataset);
	}
      std::cout << " - > Objects " << i << std::endl;
      dataset.Print();
      gmms[i]->initEM_TimeSplit(this->nstates,dataset);
      gmms[i]->doEM(dataset);
    }
 if(nbJoints > 0)
    {
        g_joints.initEM_TimeSplit(this->nstates,joints_dataset); // initialize the model
	std::cout << " start EM on joints angles" << std::endl;
	g_joints.doEM(joints_dataset); // performs EM
    }
}


void Gesture::getalldemos(Matrix * dataset)
{
  for(int i=0;i<nObjects;i++)
    {
      dataset[i].Resize(0,0);
      for(int j=0;j<ndemos[i];j++)
	{
	  dataset[i] = demos[i][j].VCat(dataset[i]);
	}
    }
  //return dataset;
  if(nbJoints > 0)
    {
        g_joints.initEM_TimeSplit(this->nstates,joints_dataset); // initialize the model
	std::cout << " start EM on joints angles" << std::endl;
	g_joints.doEM(joints_dataset); // performs EM
    }
}

void Gesture::init_trajectory(void)
{
  sigmas = new Matrix*[nObjects];
  means = new Matrix[nObjects];
  
  Vector inC(1), outC(nbVar-1);

  inC(0)=0; // Columns of the input data for regression (here, time)

  for(int i=0;i < (nbVar-1);i++) 
    outC(i)=(float)(i+1); // Columns for output : remainings

  Matrix inData(nbData,1); // generating time base .. 
  for(int i=0;i<nbData;i++) 
    inData(i,0) = i * ((float) n_resample/nbData);

  for(int i=0;i<nObjects;i++)
    {
      //std::cout << "Compute P(x,dx|t).."<< i << std::endl; 
      sigmas[i] = new Matrix[nbData];
      means[i] = gmms[i]->doRegression(inData,sigmas[i],inC,outC);
      
    }
  if(nbJoints > 0)
    {
      Vector outCJoints(nbJoints);
      Matrix * outSigmaJoints = new Matrix[nbData];
      for(int i=0;i<nbJoints;i++)
	outCJoints(i) = i+1;
      outJoints = g_joints.doRegression(inData,outSigmaJoints,inC,outCJoints); 
      delete [] outSigmaJoints; // we don't care about keeping those covariances matrices
    }
}




void Gesture::save_reference_trajectories(Matrix4 * objPositions)
{
  Matrix tMu(nstates,3);
  Matrix tCovs[nstates];
  Matrix means_t[nObjects];
  Matrix prod_t(nbData,nbVarPos);
  Matrix ** sigmas_t = new Matrix*[nObjects];
  Vector posId(nbVarPos),speedId(nbVarPos);
  Vector posId2(nbVarPos);
  
  for(int i=0;i<nbVarPos;i++) 
    {
      posId(i)=i;
      posId2(i) = i+1;
      speedId(i)=i+nbVarPos;
    } 
  for(int j=0;j<nObjects;j++)
    {
      means_t[j].Resize(nbData,3);
      sigmas_t[j] = new Matrix[nbData];
      for(int i=0;i<nbData;i++)
	{

	  Matrix rot = objPositions[j].GetOrientation();
	  Vector3 m; 
	  m.Set(means[j].GetRow(i).GetSubVector(0,nbVarPos));
	  m = objPositions[j].Transform(m);
	  Matrix c = rot*sigmas[j][i].GetMatrixSpace(posId,posId)*rot.Transpose();
	  means_t[j].SetRow(m,i);
	  sigmas_t[j][i] = c;
	}
    }
  for(int j=0;j<nstates;j++)
    {
      Matrix rot = objPositions[0].GetOrientation();
      Vector3 m;
      m.Set(gmms[0]->mu.GetRow(j).GetSubVector(1,nbVarPos));
      m = objPositions[0].Transform(m);
      tMu.SetRow(m,j);
      Matrix c = gmms[0]->sigma[j].GetMatrixSpace(posId2,posId2);
      tCovs[j] = rot*c*rot.Transpose();
    }
  
  gmms[0]->saveMuAndSigma("data/gesture_mu_abs.txt","data/gesture_cov_abs.txt",tMu,tCovs);
  /*
  if(nObjects>1)
    {
      for(int i=0;i<nbData;i++)
	{
	  Vector m;
	  Matrix sigma;
	  GaussProduct(means_t[0].GetRow(i),sigmas_t[0][i],
		       means_t[1].GetRow(i),sigmas_t[1][i],
		       m,sigma);
	  prod_t.SetRow(m,i);
	}
    }

  // demos .. 
  for(int i=0;i<nObjects;i++)
    {
      Matrix demo_all(nbData*ndemos[i],nbVarPos+1);
      int curs = 0;
      for(int j=0;j<ndemos[i];j++)
	{
	  for(int k=0;k<nbData;k++)
	    {
	      Vector m = demos[i][j].GetRow(k);
	      Vector3 p;
	      p.Set( m.GetSubVector(1,nbVarPos));
	      p = objPositions[i].Transform(p);
	      m.SetSubVector(1,p);
	      demo_all.SetRow(m,curs);
	      curs++;
	    }
	}
      char fname[200];
      sprintf(fname,"data/demo_all%02d",i);
      saveDataFile(fname,demo_all);
    }
  */
  gmms[0]->saveMuAndSigma("data/reference_arm.txt","data/reference_arm_cov.txt"
			  ,means_t[0],sigmas_t[0]);
  /* if(nObjects > 1)
    {
      saveDataFile("reference_head.txt",means_t[1]);
      saveDataFile("reference_product.txt",prod_t);
      }*/
}

Vector3 Gesture::trajectory_step_nohmm(Vector3 position)
{
  
  double sum_p;
  double p[nbData];
  Vector posId(nbVarPos),velId(nbVarPos);
  Vector currVel(nbVarPos);
  Vector vec; 


  for(unsigned i=0; i<nbVarPos; i++){
    posId[i] = i;
    velId[i] = nbVarPos+i;
  }   

  Vector posTmp[nbData],velTmp[nbData];
  Matrix covPosTmp[nbData],covVelTmp[nbData];
  sum_p=0.0;
  for(unsigned int j=0; j<nbData; j++){
    vec = means[0].GetRow(j);
    posTmp[j] = vec.GetSubVector(0,nbVarPos);
    velTmp[j] = vec.GetSubVector(nbVarPos,nbVarPos);
    covPosTmp[j] = sigmas[0][j].GetMatrixSpace(posId,posId);
    covVelTmp[j] = sigmas[0][j].GetMatrixSpace(velId,velId);
    p[j] = GaussianMixture::GaussianPDF(position, posTmp[j], covPosTmp[j]);
    sum_p += p[j];
  }
  for(unsigned int j=0; j<nbData; j++){
    p[j] = p[j]/sum_p;
  
  }

    //compute traj influence
  Vector velTraj(nbVarPos);
  Matrix covTraj(nbVarPos,nbVarPos),invcovTraj(nbVarPos,nbVarPos);
  for(unsigned int j=0; j<nbData; j++){
    velTraj += (posTmp[j]-position)*p[j]; 
    covTraj += covPosTmp[j]*(p[j]*p[j]);
  }
      
    //compute dyn influence
  Vector velDyn(nbVarPos);
  Matrix covDyn(nbVarPos,nbVarPos),invcovDyn(nbVarPos,nbVarPos);
  for(unsigned int j=0; j<nbData; j++){
    velDyn += velTmp[j]*p[j]; 
    covDyn += covVelTmp[j]*(p[j]*p[j]);
  }

  velDyn  = velDyn;    
  currVel = velDyn + velTraj*0.1;

  //thresholding the veloctiy 
  
  if(currVel.Norm() > SPEED_THRESH){
    currVel = currVel * SPEED_THRESH/currVel.Norm();
  }
  Vector3 result;
  result.Set(currVel);
  return result;

}


Vector3 Gesture::trajectory_step(int& pstate,Vector3 CurrentPosition,Matrix4 * objectsPos)
{
  Vector3 relativePosition[nObjects];
  Matrix4 inv[nObjects];
  Matrix3 rot[nObjects];

  Vector speed[nObjects];
  Matrix Cov[nObjects];

  Vector traj,dyn;
  Vector currentSpeed;
  Matrix cov;

  Vector posId(nbVarPos),speedId(nbVarPos);
  
  for(int i=0;i<nbVarPos;i++) 
    {
      posId(i)=i;
      speedId(i)=i+nbVarPos;
    } 

  // get relative Position to each object 
  
  for(int i=0;i<nObjects;i++)
    {
      rot[i] = objectsPos[i].GetOrientation();
      objectsPos[i].InverseTransformation(inv[i]);
      relativePosition[i] = inv[i].Transform(CurrentPosition);
    }

  if(pstate == -1)
    {
      float p;
      float maxp=0.;
      int s;
      for(int j = 0;j<nbData;j++)
	{
	  p = 0.;
	  for(int i=0;i<nObjects;i++)
	    {
	      // check if we can go to next state 

	      traj = means[i].GetRow(j).GetSubVector(0,nbVarPos);
	      cov = sigmas[i][j].GetMatrixSpace(posId,posId);
	      
	      p += GaussianMixture::GaussianPDF(relativePosition[i],traj,cov);
	    }
	  if(p>maxp)
	    {
	      maxp = p;
	      s = j;
	    }
	}
      pstate = s;
    }

  if(pstate < nbData -1)
    {
      float cp[nObjects],np[nObjects];
      for(int i=0;i<nObjects;i++)
	{
	  // check if we can go to next state 

	  traj = means[i].GetRow(pstate).GetSubVector(0,nbVarPos);
	  cov = sigmas[i][pstate].GetMatrixSpace(posId,posId);
	  
	  cp[i] = GaussianMixture::GaussianPDF(relativePosition[i],traj,cov);
	  
	  traj = means[i].GetRow(pstate + 1).GetSubVector(0,nbVarPos);
	  cov = sigmas[i][pstate + 1].GetMatrixSpace(posId,posId);
	  
	  np[i] = GaussianMixture::GaussianPDF(relativePosition[i],traj,cov);
	
	}
      
      if( (np[0] > cp[0]*0.5) &&(np[1] > cp[1]*0.5))
	pstate++;
    }
      // compute speed 
   for(int i=0;i<nObjects;i++)
    {  
      traj = means[i].GetRow(pstate).GetSubVector(0,nbVarPos);
      traj = traj-relativePosition[i];
      cov = sigmas[i][pstate].GetMatrixSpace(posId,posId);
      dyn = means[i].GetRow(pstate).GetSubVector(nbVarPos,nbVarPos);

      dyn = dyn*200;
      speed[i] = rot[i] * (traj * BETA + dyn * (1-BETA));

      Cov[i] = rot[i] * cov * rot[i].Transpose();
    }

   if(nObjects == 2)
     GaussProduct(speed[0],Cov[0],speed[1],Cov[1],currentSpeed,cov);
   else
     {
       currentSpeed = speed[0];
       cov = Cov[0];
     }
  if(currentSpeed.Norm() > SPEED_THRESH)
    {
      currentSpeed = currentSpeed * SPEED_THRESH / currentSpeed.Norm();
    }
  
  return (Vector3)currentSpeed;
}

void Gesture::init_alpha(void)
{
  this->alpha = new double[nbData];
  for(int j=0;j<nbData;j++)
    this->alpha[j]=1./(j+1);
}

void Gesture::init_dynamics(Vector CurrentPosition)
{
  this->previous_position = CurrentPosition;
  this->previous_speed.Resize(nbVarPos);
  this->previous_speed.Zero();
  this->previous_speed.Print();
}

Vector Gesture::trajectory_step_dyn(Vector CurrentPosition)
{
  Vector speed = this->previous_speed;
  Vector desired_position(nbVarPos);
  Vector desired_speed(nbVarPos);
  double weights[nbData];
  double sum_weights=0;
  Vector input(6);
  Vector posId(nbVarPos),speedId(nbVarPos);
  Vector allId(nbVarPos*2);
  
  for(int i=0;i<nbVarPos;i++) 
    {
      posId(i)=i;
      speedId(i)=i+nbVarPos;
    } 
  for(int i=0;i<2*nbVarPos;i++)
    allId(i) = i;

  input.SetSubVector(0,CurrentPosition);
  input.SetSubVector(nbVarPos,speed);
     
  for(int i=0;i<nbData;i++)
    {
      Vector mu = means[0].GetRow(i);
      Matrix sigma = sigmas[0][i];     
      weights[i] = GaussianMixture::GaussianPDF(input,mu,sigma);
      sum_weights += weights[i];
    }
  if(sum_weights == 0)
    {
      std::cout << "oops " << std::endl;
    }
  for(int i=0;i<nbData;i++)
    {
      weights[i] /= sum_weights;
      Vector mu = means[0].GetRow(i);
      Matrix sig = sigmas[0][i];
      Matrix sig_x = sig.GetMatrixSpace(posId,posId);
      Matrix sig_s = sig.GetMatrixSpace(speedId,speedId);
      Matrix sig_xs = sig.GetMatrixSpace(posId,speedId);
      Vector m_pos = mu.GetSubVector(0,nbVarPos);
      Vector m_speed = mu.GetSubVector(nbVarPos,nbVarPos);
      desired_position += (m_pos + sig_xs* sig_s.Inverse() * (speed-m_speed))*weights[i];
      /*desired_speed += (m_speed + sig_xs.Transpose()*sig_x.Inverse()*
			(CurrentPosition-m_pos)) * weights[i];
      
			desired_position += (m_pos )*weights[i];*/
      desired_speed += (m_speed ) * weights[i];

    }
  //desired_position.Print();
  //CurrentPosition.Print();
  Vector acc_position = ((desired_position - CurrentPosition)*0.06 - speed)*0.2;
  Vector acc_speed = (desired_speed-speed)*0.3;
 
  Vector n_speed = speed +  acc_position + acc_speed ;
  this->previous_speed = n_speed;
  //std::cout << acc_position.Norm() << "  " <<acc_speed.Norm() << std::endl;
  if(n_speed.Norm() > SPEED_THRESH)
    n_speed = n_speed * SPEED_THRESH / n_speed.Norm();
  return n_speed;
}

Vector3 Gesture::trajectory_step_hmm(Vector3 CurrentPosition) 
{
  Matrix4 fakeObj[nObjects];
  for(int i=0;i<nObjects;i++)
    fakeObj[i] = Matrix4::IDENTITY;
  return this->trajectory_step_hmm(CurrentPosition,fakeObj);
}

Vector3 Gesture::trajectory_step_hmm(Vector3 CurrentPosition,Matrix4 * objectsPos)
{
  Vector relativePosition[nObjects];
  Matrix4 inv[nObjects];
  Matrix3 rot[nObjects];

  Vector speed[nObjects];
  Matrix Cov[nObjects];

  Vector traj(3),dyn(3);
  Vector currentSpeed;
  Matrix cov(3,3);
  double sum_weights = 0.;
  double sum_alpha = 0.;
  double p[nbData];
  Vector posId(nbVarPos),speedId(nbVarPos);
  
  for(int i=0;i<nbVarPos;i++) 
    {
      posId(i)=i;
      speedId(i)=i+nbVarPos;
    } 

  for(int i=0;i<nObjects;i++)
    {
      rot[i] = objectsPos[i].GetOrientation();
      objectsPos[i].InverseTransformation(inv[i]);
      relativePosition[i] = inv[i].Transform(CurrentPosition);
    }
  // get relative Position to each object 
  
  this->alpha[0] = this->alpha[0]*0.5;
  for(int j=1;j<nbData;j++)
    {
      this->alpha[j] = this->alpha[j]*0.5 + this->alpha[j-1]*0.5;
    }
    

  for(int j = 0;j<nbData;j++)
    {
      p[j]=0;
      for(int i=0;i<nObjects;i++)
	{
	  // check if we can go to next state 
	  
	  traj = means[i].GetRow(j).GetSubVector(0,nbVarPos);
	  cov = sigmas[i][j].GetMatrixSpace(posId,posId);
	  
	  p[j] += GaussianMixture::GaussianPDF(relativePosition[i],traj,cov);
	}
      this->alpha[j] *= p[j];
      sum_alpha += this->alpha[j];
      sum_weights += this->alpha[j]*p[j];
    }


  for(int j=0;j<nbData;j++)
    {
      p[j] *= this->alpha[j]/sum_weights;
      this->alpha[j] /= sum_alpha;
    }

  for(int i=0;i<nObjects;i++)
    { 
      traj.Zero();
      cov.Zero();
      dyn.Zero();
      
      for(int j=0;j<nbData;j++)
	{
	  Vector  t = means[i].GetRow(j).GetSubVector(0,nbVarPos);
	  traj += (t-relativePosition[i]) * p[j];
	  cov += sigmas[i][j].GetMatrixSpace(posId,posId)*p[j];
	  dyn += means[i].GetRow(j).GetSubVector(nbVarPos,nbVarPos)* p[j];
	  // dyn.Print();
	}
	  
      // dyn = dyn*200;
      // std::cout << traj.Norm() << "  " << dyn.Norm() << std::endl;
      speed[i] = rot[i] * (traj * BETA + dyn * (1-BETA));
	  
	  Cov[i] = rot[i] * cov * rot[i].Transpose();
    }

   if(nObjects == 2)
     GaussProduct(speed[0],Cov[0],speed[1],Cov[1],currentSpeed,cov);
   else
     {
       currentSpeed = speed[0];
       cov = Cov[0];
     }
  if(currentSpeed.Norm() > SPEED_THRESH)
    {
      currentSpeed = currentSpeed * SPEED_THRESH / currentSpeed.Norm();
    }
  return (Vector3)currentSpeed;
}

/* return reference posture corresponding to 
   latest trajectory_step_hmm */
Vector Gesture::getReferencePosture(void)
{
  Vector posture(nbJoints);
  for(int i = 0;i<nbData;i++)
    {
      posture += outJoints.GetRow(i)*this->alpha[i];
    }
  return posture;
}
    



int GaussProduct(Matrix Mu1, Matrix * Sigma1,
		 Matrix Mu2, Matrix * Sigma2,
		 Matrix& Mu, Matrix * Sigma)
{
  int nData = Mu1.RowSize();
  Mu.Resize(nData,Mu1.ColumnSize());
  //Sigma = new Matrix[nData];
  for(int i=0;i<nData;i++)
    {
      Vector tMu(Mu1.ColumnSize());
      Sigma[i].Resize(Mu1.ColumnSize(),Mu1.ColumnSize() );
      GaussProduct(Mu1.GetRow(i),Sigma1[i],
		   Mu2.GetRow(i),Sigma2[i],
		   tMu,Sigma[i]);
      Mu.SetRow(tMu,i);
    }
  return 1;
}

int GaussProduct(Vector Mu1,Matrix Sigma1,Vector Mu2,Matrix Sigma2,Vector& Mu, Matrix& Sigma)
{
  Matrix inv1,inv2,inv;
  Sigma1.Inverse(inv1);
  Sigma2.Inverse(inv2);
  Sigma = inv1 + inv2;
  Sigma.Inverse(inv);
  if(Sigma.IsInverseOk()){
    Sigma = inv;
    Mu = inv*(inv1*Mu1  + inv2*Mu2);
    return 1;
  }
 
    std::cout << "fffuuuuuck" <<std::endl;
    return -1;
 
}
